#include "gaoqing_motor_hardwareinterface/socketcanfd.hpp"

#include <cmath>
#include <vector>

namespace gaoqing_motor_hardware_interface
{
gaoqing_motor_socketcanfd::gaoqing_motor_socketcanfd()
{
    
}
gaoqing_motor_socketcanfd::~gaoqing_motor_socketcanfd()
{
    gaoqing_motor_shutdown(gaoqing_motor_id_);
}

bool gaoqing_motor_socketcanfd::init(const char* ifname)
{
    // 创建socket
    if ((socket_fd_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
        perror("Error while opening socket");
        return false;
    }
    // 设置CAN接口名称
    std::strcpy(ifr_.ifr_name, ifname);
    if (ioctl(socket_fd_, SIOCGIFINDEX, &ifr_) < 0) {
        perror("Error getting interface index");
        close(socket_fd_);
        return false;
    }
    // 绑定socket到CAN接口
    addr_.can_family = AF_CAN;
    addr_.can_ifindex = ifr_.ifr_ifindex;
    if (bind(socket_fd_, (struct sockaddr *)&addr_, sizeof(addr_)) < 0) {
        perror("Error binding socket");
        close(socket_fd_);
        return false;
    }
    // 启用canfd
    int enable_canfd = 1;
    if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, sizeof(enable_canfd)) < 0) {
        perror("Error enabling CAN-FD");
        close(socket_fd_);
        return false;
    }

    return true;
}

bool gaoqing_motor_socketcanfd::gaoqing_motor_shutdown(uint16_t id)
{
    struct canfd_frame sframe;
    struct canfd_frame rframe;
    sframe.can_id = id;
    sframe.len = 3;
    sframe.flags = CANFD_FDF|CANFD_BRS;
    sframe.data[0] = 0x01U;
    sframe.data[1] = 0x00U;
    sframe.data[2] = 0x00U;

    if (write(socket_fd_, &sframe, sizeof(struct canfd_frame)) < 0) {
        perror("Error writing to socket");
        return false;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return false;
    }
    return true;
}

void gaoqing_motor_socketcanfd::gaoqing_motor_torque_control(int16_t torque1)
{
    
}

void gaoqing_motor_socketcanfd::gaoqing_motor_state_read(uint16_t id)
{
    struct canfd_frame sframe;
    struct canfd_frame rframe;
    sframe.can_id = 0x8000|id;
    sframe.len = 3;
    sframe.flags = CANFD_FDF|CANFD_BRS;
    sframe.data[0] = 0x1CU;
    sframe.data[1] = 0x04U;
    sframe.data[2] = 0x00U;
    if (write(socket_fd_, &sframe, sizeof(struct canfd_frame)) < 0) {
        perror("Error writing to socket");
        return;
    }
    if (read(socket_fd_, &rframe, sizeof(struct can_frame)) < 0) {
        perror("Error reading from socket");
        return;
    }
    if (rframe.data[0] == 0x2CU) // cmd: 0010 1100
    {
        motor_state_[id - 1].position = static_cast<float>(rframe.data[3] + rframe.data[4]<<8);
        motor_state_[id - 1].velocity = static_cast<float>(rframe.data[5] + rframe.data[6]<<8);
        motor_state_[id - 1].torque = static_cast<float>(rframe.data[7] + rframe.data[8]<<8);
    }

}

}